#include "C_H.h"
// 全局变量定义
cv::VideoCapture cap;
cv::Mat frame, gray_frame, compressed;  // 定义为全局变量
int sock;  // 添加socket全局变量定义
uint8_t Pixle[LCDH][LCDW];
ImageDealDatatype ImageDeal[120];
ImageStatustype ImageStatus;
ImageFlagtypedef ImageFlag;
SystemDatatype SystemData = {{0.0, 0}};
int Xsite, Ysite;
int BottomBorderLeft, BottomBorderRight, BottomCenter;
float DetL, DetR;
int FTSite, TFSite;
uint8_t ExtenRFlag = 0, ExtenLFlag = 0;
uint8_t* PicTemp;
int ytemp;
uint8 Ring_Help_Flag = 0;                      // 圆环辅助标志
int Left_RingsFlag_Point1_Ysite, Left_RingsFlag_Point2_Ysite;   // 左圆环判断点纵坐标
int Right_RingsFlag_Point1_Ysite, Right_RingsFlag_Point2_Ysite; // 右圆环判断点纵坐标
int Point_Xsite,Point_Ysite;                   // 终点横纵坐标
int Repair_Point_Xsite,Repair_Point_Ysite;     // 修复点横纵坐标
int CenterLine[LCDH];                          // 存储每一行的中线位置
// 添加中线预测相关变量
int PredictedCenter[LCDH];  // 存储预测的中线位置
bool EnablePrediction = true; // 启用预测功能开关

// 添加传输数据结构体
struct TransmitData {
    float deviation;       // 偏差值
    int16_t encoder_left;  // 左编码器值
    int16_t encoder_right; // 右编码器值
};

// 图像处理相关变量
int IntervalLow = 0;
int IntervalHigh = 0;
float Weighting[10] = {0.96, 0.92, 0.88, 0.83, 0.77, 0.71, 0.65, 0.59, 0.53, 0.47}; // 10行权重参数

// 控制相关变量
PIDController pid;
IncrementalPIDController incremental_pid; // 创建增量式 PID 控制器实例

// 电机相关定义
#define MOTOR1_DIR   "/dev/zf_driver_gpio_motor_1"
#define MOTOR1_PWM   "/dev/zf_device_pwm_motor_1"
#define MOTOR2_DIR   "/dev/zf_driver_gpio_motor_2"
#define MOTOR2_PWM   "/dev/zf_device_pwm_motor_2"
#define ESC_PWM_1    "/dev/zf_device_pwm_esc_1"
#define ESC_PWM_2    "/dev/zf_device_pwm_esc_2"
// 舵机相关定义
#define SERVO_PWM    "/dev/zf_device_pwm_servo"

// 编码器定义
#define ENCODER_1    "/dev/zf_encoder_1"
#define ENCODER_2    "/dev/zf_encoder_2"
int16_t encoder_left;
int16_t encoder_right;

// PWM信息结构体
struct pwm_info motor_1_pwm_info;
struct pwm_info motor_2_pwm_info;
struct pwm_info servo_pwm_info;

// PWM最大值定义
#define MOTOR1_PWM_DUTY_MAX    (motor_1_pwm_info.duty_max)       
#define MOTOR2_PWM_DUTY_MAX    (motor_2_pwm_info.duty_max)        
#define SERVO_PWM_DUTY_MAX     (servo_pwm_info.duty_max)

// 舵机限幅参数
#define SERVO_MID_DUTY      (1195)    // 舵机中值     自己给值就行

// 控制参数和状态变量
float current_angle = 40;  // 舵机打角变量
float Det_True = 0.0;      // 偏差值
float time_lock;           // 时间锁
float dajiao;              // 打角值
int wan_cnt = 0;          // 弯道计数
float setPoint = 0.0;     // 设定点
float show_flag;          // 显示标志
float nts_flag;           // NTS标志
int Road_flag = 0;        // 道路标志

// 显示相关定义
cv::Mat display_buffer;    // 显示缓冲区
const int DISPLAY_WIDTH = 80;
const int DISPLAY_HEIGHT = 60;
const int DISPLAY_CHANNELS = 3;  // BGR格式

// 速度相关定义
const float Speed_Diffen_xishu = 0.25f;  // 差速系数
float Speed_Goal = 360.0f;               // 目标速度
float Speed_Goal_l = 360.0f;                  // 左轮目标速度
float Speed_Goal_r = 360.0f;                  // 右轮目标速度
float S3010_Duty = SERVO_PWM_DUTY_MAX / 2;  // 舵机PWM值，初始化为中值

// 在全局变量区域添加
std::atomic<bool> should_exit{false};  // 程序退出标志
uint8 Half_Road_Wide[60] =                      // 直道宽度数组
{  4, 5, 5, 6, 6, 6, 7, 7, 8, 8,
        9, 9,10,10,10,11,12,12,13,13,
       13,14,14,15,15,16,16,17,17,17,
       18,18,19,19,20,20,20,21,21,22,
       23,23,23,24,24,25,25,25,26,26,
       27,28,28,28,29,30,31,31,31,32,
};

uint8 Half_Bend_Wide[60] =                      // 弯道宽度数组
{   33,33,33,33,33,33,33,33,33,33,
    33,33,32,32,30,30,29,29,28,27,
    28,27,27,26,26,25,25,24,24,23,
    22,21,21,22,22,22,23,24,24,24,
    25,25,25,26,26,26,27,27,28,28,
    28,29,29,30,30,31,31,32,32,33,
};

uint8 buxianwide[60]={6,7,8,9,10,11,12,13,14,15,
                      16,17,18,19,20,21,22,23,24,25,
                      26,27,28,29,30,31,32,33,34,35,
                      36,37,38,39,40,41,42,43,44,45,
                      46,47,48,49,50,51,52,53,54,55,
                      56,57,58,59,60,61,62,63,64,65};
                      
// 在全局变量区域添加配置参数
struct Config {
    float Kp = 1.3f;
    float Ki = 0.0f;
    float Kd = 4.0f;
    int pwm = 2500;
    int enable = 0;
    float time_lock = 7.0f;
    float dajiao = 25.0f;
    
    // 直弯分离参数
    float Kp_zhi = 0.0f;
    float Kd_zhi = 0.0f;
    float Kp_wan = 0.0f;
    float Kd_wan = 0.0f;
    
    float nts_flag = 0.0f;
    int show_flag = 1;
    int image_trans = 0;  // 图传标志：0-关闭图传，1-开启图传
} config;

// 一阶低通滤波器类
class LowPassFilter {
private:
    float alpha = 0.2f;  // 滤波系数 (0-1)，越小滤波效果越强
    float last_value = 0.0f;
    bool is_first = true;

public:
    // 重置滤波器
    void reset() {
        last_value = 0.0f;
        is_first = true;
    }

    // 滤波处理
    float filter(float input) {
        if (is_first) {
            last_value = input;
            is_first = false;
            return input;
        }
        
        // 一阶低通滤波公式：y(n) = α * x(n) + (1-α) * y(n-1)
        float output = alpha * input + (1.0f - alpha) * last_value;
        last_value = output;
        return output;
    }

    // 设置滤波系数
    void setAlpha(float new_alpha) {
        if (new_alpha >= 0.0f && new_alpha <= 1.0f) {
            alpha = new_alpha;
        }
    }
};

// 为左右编码器创建滤波器实例
LowPassFilter leftEncoderFilter;
LowPassFilter rightEncoderFilter;

// 函数声明
void speed_dif_jun(void);
void motor_control_thread_entry(void);
void servo_control_thread_entry(void);
bool initCamera(void);
void ImageProcess(void);
void ZhiWan_Judge_yuanduan(void);

void motor_control_callback()
{
    // 添加详细的错误处理
    int16_t left_count = encoder_get_count(ENCODER_1);
    int16_t right_count = encoder_get_count(ENCODER_2);

    // 更新编码器值
    encoder_left = -left_count;
    encoder_right = -right_count;

    // 打印编码器值
    std::cout << "编码器值 - 左: " << encoder_left << ", 右: " << encoder_right << std::endl;

    // 计算当前速度（使用最新的有效值）
    float current_speed_l = encoder_left;
    float current_speed_r = encoder_right;


    // 进行 PID 控制
    int32 pid_output_l = incremental_pid.calculate(current_speed_l, Speed_Goal_l);
    int32 pid_output_r = incremental_pid.calculate(current_speed_r, Speed_Goal_r);

    // 限幅处理
    if(pid_output_l > 6000) pid_output_l = 6000;
    else if(pid_output_l < -6000) pid_output_l = -6000;
    if(pid_output_r > 6000) pid_output_r = 6000;
    else if(pid_output_r < -6000) pid_output_r = -6000;
    // gpio_set_level(MOTOR1_DIR, 1);  // 正转
    // pwm_set_duty(MOTOR1_PWM, 2000);
    // gpio_set_level(MOTOR2_DIR, 1);  // 正转s
    // pwm_set_duty(MOTOR2_PWM, 2000);
    // 设置 PWM 输出和方向
    // 左电机控制
    if (pid_output_l >= 0) {
        gpio_set_level(MOTOR1_DIR, 1);  // 正转
        pwm_set_duty(MOTOR1_PWM, pid_output_l);
    } else {
        gpio_set_level(MOTOR1_DIR, 0);  // 反转
        pwm_set_duty(MOTOR1_PWM, (-pid_output_l));  // 取绝对值
    }

    // 右电机控制
    if (pid_output_r >= 0) {
        gpio_set_level(MOTOR2_DIR, 0);  // 正转
        pwm_set_duty(MOTOR2_PWM, pid_output_r);
    } else {
        gpio_set_level(MOTOR2_DIR, 1);  // 反转
        pwm_set_duty(MOTOR2_PWM, (-pid_output_r));  // 取绝对值
    }
}

void esc_control_callback()
{

    if (should_exit) {
        pwm_set_duty(ESC_PWM_1, 0);
        pwm_set_duty(ESC_PWM_2, 0);
    } else {
        pwm_set_duty(ESC_PWM_1, 650);
        pwm_set_duty(ESC_PWM_2, 650);
    }
}

// 舵机控制回调函数
void servo_control_callback()
{
    static bool first_run = true;
    if (first_run) {
        std::cout << "舵机控制器已启动" << std::endl;
        first_run = false;
    }

    float current_deviation = current_angle;
    int16 pid_output = pid.calculate(current_deviation);

    // 在中值基础上添加PID输出
    int16 servo_duty = SERVO_MID_DUTY + static_cast<int>(pid_output);
    
    // 设置舵机PWM
    S3010_Duty = servo_duty;
    pwm_set_duty(SERVO_PWM, servo_duty);
}

// 初始化摄像头
bool initCamera() {
    cap.open(0);
    if (!cap.isOpened()) {
        std::cerr << "错误：无法打开摄像头" << std::endl;
        return false;
    }
    
    // 设置摄像头参数为320*240
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 160);  // 设置宽度为320
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 120); // 设置高度为240
    cap.set(cv::CAP_PROP_FPS, 90);
    
    // 预分配内存
    frame = cv::Mat(120, 160, CV_8UC3);      // 更新为320x240
    gray_frame = cv::Mat(120, 160, CV_8UC1); // 更新为320x240
    compressed = cv::Mat(LCDH, LCDW, CV_8UC1);
    
    return true;
}

// 裁剪图像的函数
cv::Mat cropImage(const cv::Mat& input_frame) {
    // 计算裁剪的边界
    int height = input_frame.rows;
    int width = input_frame.cols;

    // 使用 Range 裁剪图像，裁剪掉上面的240个像素
    return input_frame(cv::Range(60, height), cv::Range(0, width)); // 保留从60到height的部分
}

// 图像获取和压缩
void getCompressedImage() {
    if (!cap.read(frame)) {
        std::cerr << "错误：无法读取图像" << std::endl;
        return;
    } 

    // 检查图像是否有效
    if (frame.empty()) {
        std::cerr << "错误：读取的图像为空" << std::endl;
        return;
    }

    // 裁剪图像
    frame = cropImage(frame);  // 调用裁剪函数

    // 检查裁剪后的图像是否有效
    if (frame.empty()) {
        std::cerr << "错误：裁剪后的图像为空" << std::endl;
        return;
    }

    // 直接压缩到80x60
    cv::resize(frame, gray_frame, cv::Size(80, 60), 0, 0, cv::INTER_LINEAR);
    
    // 转换为灰度图
    cv::cvtColor(gray_frame, compressed, cv::COLOR_BGR2GRAY);
    
    // 进行二值化处理
    cv::threshold(compressed, compressed, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);

    // 更新Pixle数组
    for (int i = 0; i < 60; i++) {
        for (int j = 0; j < 80; j++) {
            Pixle[i][j] = (compressed.at<uchar>(i, j) > 127) ? 1 : 0;
        }
    }
}

// 限制函数
void LimitL(int& value) {
    if (value < 0) value = 0;
}

void LimitH(int& value) {
    if (value > 79) value = 79;
}
/*****************ֱ   ж ******************/
float Straight_Judge(uint8 flag)//返回结果小于1即为直线
{
    float out=0;
    int X_temp[12],Y_temp[12],i=0,Point=1,j=1;
    float K_temp,K_other[12];
    if(flag==1)//左边线平均斜率计算
    {
        for(i=ImageStatus.OFFLine+5;i<30;i++)
        {
            if((ImageDeal[i].IsLeftFind=='T')&&(ImageDeal[i].LeftBorder>2))
            {
                X_temp[0]=ImageDeal[i].LeftBorder;
                Y_temp[0]=i;
                break;
            }
            if(i>=19)return 1;
        }
        i+=5;
        for(;i<50;i=i+5)
        {
            if((ImageDeal[i].IsLeftFind=='T')&&(ImageDeal[i].LeftBorder>2))
            {
                Point++;
                X_temp[j]=ImageDeal[i].LeftBorder;
                Y_temp[j]=i;
                j++;
            }
        }
        if(Point<5)return 1;
        else
        {
            K_temp=(float)(X_temp[0]-X_temp[Point-1])/(float)(Y_temp[Point-1]-Y_temp[0]);
            for(i=1;i<Point-1;i++)
            {
                K_other[i]=(float)(X_temp[0]-X_temp[i])/(float)(Y_temp[i]-Y_temp[0]);
                out+=(K_temp-K_other[i])*(K_temp-K_other[i]);
            }
        }
        if(K_temp<0)return 1;
    }
    else//右边线平均斜率计算
    {
        for(i=ImageStatus.OFFLine+5;i<30;i++)
        {
            if((ImageDeal[i].IsRightFind=='T')&&(ImageDeal[i].RightBorder<77))
            {
                X_temp[0]=ImageDeal[i].RightBorder;
                Y_temp[0]=i;
                break;
            }
            if(i>=19)return 1;
        }
        i+=5;
        for(;i<50;i=i+5)
        {
            if((ImageDeal[i].IsRightFind=='T')&&(ImageDeal[i].RightBorder<77))
            {
                Point++;
                X_temp[j]=ImageDeal[i].RightBorder;
                Y_temp[j]=i;
                j++;
            }
        }
        if(Point<5)return 1;
        else
        {
            K_temp=(float)(X_temp[0]-X_temp[Point-1])/(float)(Y_temp[0]-Y_temp[Point-1]);
            for(i=1;i<Point-1;i++)
            {
                K_other[i]=(float)(X_temp[0]-X_temp[i])/(float)(Y_temp[0]-Y_temp[i]);
                out+=(K_temp-K_other[i])*(K_temp-K_other[i]);
            }
        }
        if(K_temp<0)return 1;
    }
    return out;
}


void GetJumpPointFromDet(uint8_t* p,uint8_t type,int L,int H,JumpPointtypedef* Q)  // 第一个参数是要查找的数组（80个点）
                                                                                    // 第二个参数是扫左边线还是右边线
                                                                                    // 第三四个参数是开始和结束点
{                                                                              //三四是开始和结束点
  int i = 0;
  if (type == 'L')                              //扫描左边线
  {
    for (i = H; i >= L; i--) {
      if (*(p + i) == 1 && *(p + i - 1) != 1)   //由黑变白
      {
        Q->point = i;                           //记录左边线
        Q->type = 'T';                          //正确跳变
        break;
      } else if (i == (L + 1))                  //若果扫到最后也没找到
      {
        if (*(p + (L + H) / 2) != 0)            //如果中间是白的ss
        {
          Q->point = (L + H) / 2;               //认为左边线是中点
          Q->type = 'W';                        //非正确跳变且中间为白，认为没有边
          break;
        } else                                  //非正确跳变且中间为黑
        {
          Q->point = H;                         //如果中间是黑的
          Q->type = 'H';                        //左边线直接最大值，认为是大跳变
          break;
        }
      }
    }
  } else if (type == 'R')                       //扫描右边线
  {
    for (i = L; i <= H; i++)                    //从右往左扫
    {
      if (*(p + i) == 1 && *(p + i + 1) != 1)   //找由黑到白的跳变
      {
        Q->point = i;                           //记录
        Q->type = 'T';
        break;
      } else if (i == (H - 1))                  //若果扫到最后也没找到
      {
        if (*(p + (L + H) / 2) != 0)            //如果中间是白的
        {
          Q->point = (L + H) / 2;               //右边线是中点
          Q->type = 'W';
          break;
        } else                                  //如果中点是黑的
        {
          Q->point = L;                         //左边线直接最大值
          Q->type = 'H';
          break;
        }
      }
    }
  }
}

uint8_t DrawLinesFirst(void) {
  PicTemp = Pixle[59];
  if (*(PicTemp + ImageSensorMid) == 0)                 //如果底边图像中点为黑，异常情况
  {
    for (Xsite = 0; Xsite < ImageSensorMid; Xsite++)    //找左右边线
    {
      if (*(PicTemp + ImageSensorMid - Xsite) != 0)     //一旦找到左或右赛道到中心距离，就break
        break;                                          //并且记录Xsite
      if (*(PicTemp + ImageSensorMid + Xsite) != 0)
        break;
    }

    if (*(PicTemp + ImageSensorMid - Xsite) != 0)       //赛道如果在左边的话
    {
      BottomBorderRight = ImageSensorMid - Xsite + 1;   // 59行右边线有啦
      for (Xsite = BottomBorderRight; Xsite > 0; Xsite--)  //开始找59行左边线
      {
        if (*(PicTemp + Xsite) == 0 &&
            *(PicTemp + Xsite - 1) == 0)                //连续两个黑点，滤波
        {
          BottomBorderLeft = Xsite;                     //左边线找到
          break;
        } else if (Xsite == 1) {
          BottomBorderLeft = 0;                         //搜索到最后了，看不到左边线，左边线认为是0
          break;
        }
      }
    } else if (*(PicTemp + ImageSensorMid + Xsite) != 0)  //赛道如果在右边的话
    {
      BottomBorderLeft = ImageSensorMid + Xsite - 1;    // 59行左边线有啦
      for (Xsite = BottomBorderLeft; Xsite < 79; Xsite++)  //开始找59行左边线
      {
        if (  *(PicTemp + Xsite) == 0
            &&*(PicTemp + Xsite + 1) == 0)              //连续两个黑点，滤波
        {
          BottomBorderRight = Xsite;                    //右边线找到
          break;
        } else if (Xsite == 78) {
          BottomBorderRight = 79;                       //搜索到最后了，看不到右边线，左边线认为是79
          break;
        }
      }
    }
  }
  else                                                //左边线中点是白的，比较正常的情况
  {
    for (Xsite = 79; Xsite >ImageSensorMid; Xsite--)   //一个点一个点地搜索右边线
    {
      if (  *(PicTemp + Xsite) == 1
          &&*(PicTemp + Xsite - 1) == 1)                //连续两个黑点，滤波     //两个白点
      {
        BottomBorderRight = Xsite;                      //找到就记录
        break;
      } else if (Xsite == 40) {
        BottomBorderRight = 39;                         //找不到认为79
        break;
      }
    }
    for (Xsite = 0; Xsite < ImageSensorMid; Xsite++)    //一个点一个点地搜索左边线
    {
      if (  *(PicTemp + Xsite) == 1
          &&*(PicTemp + Xsite + 1) == 1)                //连续两个黑点，滤波
      {
        BottomBorderLeft = Xsite;                       //找到就记录
        break;
      } else if (Xsite == 38) {
        BottomBorderLeft = 39;                           //找不到认为0
        break;
      }
    }
  }
  BottomCenter =(BottomBorderLeft + BottomBorderRight) / 2;   // 59行中点直接取平均
  ImageDeal[59].LeftBorder = BottomBorderLeft;                //在数组里面记录一下信息，第一行特殊一点而已
  ImageDeal[59].RightBorder = BottomBorderRight;
  ImageDeal[59].Center = BottomCenter;                        //确定最底边
  ImageDeal[59].Wide = BottomBorderRight - BottomBorderLeft;  //存储宽度信息
  ImageDeal[59].IsLeftFind = 'T';
  ImageDeal[59].IsRightFind = 'T';
  for (Ysite = 58; Ysite > 54; Ysite--)                       //由中间向两边确定底边五行
  {
    PicTemp = Pixle[Ysite];
    for (Xsite = 79; Xsite > ImageDeal[Ysite + 1].Center;Xsite--)                                             //和前面一样的搜索
    {
      if (*(PicTemp + Xsite) == 1 && *(PicTemp + Xsite - 1) == 1) {
        ImageDeal[Ysite].RightBorder = Xsite;
        break;
      } else if (Xsite == (ImageDeal[Ysite + 1].Center+1)) {
        ImageDeal[Ysite].RightBorder = ImageDeal[Ysite + 1].Center;
        break;
      }
    }
    for (Xsite = 0; Xsite < ImageDeal[Ysite + 1].Center;Xsite++)                                             //和前面一样的搜索
    {
      if (*(PicTemp + Xsite) == 1 && *(PicTemp + Xsite + 1) == 1) {
        ImageDeal[Ysite].LeftBorder = Xsite;
        break;
      } else if (Xsite == (ImageDeal[Ysite + 1].Center-1)) {
        ImageDeal[Ysite].LeftBorder = ImageDeal[Ysite + 1].Center;
        break;
      }
    }
    ImageDeal[Ysite].IsLeftFind = 'T';                        //这些信息存储到数组里
    ImageDeal[Ysite].IsRightFind = 'T';
    ImageDeal[Ysite].Center =
        (ImageDeal[Ysite].RightBorder + ImageDeal[Ysite].LeftBorder) /2; //存储中点
    ImageDeal[Ysite].Wide =
        ImageDeal[Ysite].RightBorder - ImageDeal[Ysite].LeftBorder;      //存储宽度
  }
  return 'T';
}                                                             //最基本的要求，最近的五行首先不会受到干扰，这需要在安装的时候调整摄像头的视角

/*边线追逐大致得到全部边线*/
void DrawLinesProcess(void)  //////不用更改
{
  uint8_t L_Found_T = 'F';  //确定无边斜率的基准有边行是否被找到的标志
  uint8_t Get_L_line = 'F';  //找到这一帧图像的基准左斜率
  uint8_t R_Found_T = 'F';  //确定无边斜率的基准有边行是否被找到的标志
  uint8_t Get_R_line = 'F';  //找到这一帧图像的基准右斜率
  float D_L = 0;           //延长线左边线斜率
  float D_R = 0;           //延长线右边线斜率
  int ytemp_W_L = 0;           //记住首次左丢边行
  int ytemp_W_R = 0;           //记住首次右丢边行
  ExtenRFlag = 0;          //标志位清0
  ExtenLFlag = 0;
  ImageStatus.Left_Line = 0;
  ImageStatus.WhiteLine = 0;
  ImageStatus.Right_Line = 0;
  for (Ysite = 54 ; Ysite > ImageStatus.OFFLine; Ysite--)            //前5行处理过了，下面从55行到（设定的不处理的行OFFLine）
  {                        //太远的图像不稳定，OFFLine以后的不处理
    PicTemp = Pixle[Ysite];
    JumpPointtypedef JumpPoint[2];                                          // 0左1右
    if (ImageStatus.Road_type != Cross_ture
           /* &&SystemData.SpeedData.Length*OX>500*/) {
      IntervalLow =ImageDeal[Ysite + 1].RightBorder -ImageScanInterval;             //从上一行右边线-Interval的点开始（确定扫描开始点）
      IntervalHigh =ImageDeal[Ysite + 1].RightBorder + ImageScanInterval;           //到上一行右边线+Interval的点结束（确定扫描结束点）
    } else {
      IntervalLow =ImageDeal[Ysite + 1].RightBorder -ImageScanInterval_Cross;       //从上一行右边线-Interval_Cross的点开始（确定扫描开始点）
      IntervalHigh = ImageDeal[Ysite + 1].RightBorder + ImageScanInterval_Cross;    //到上一行右边线+Interval_Cross的点开始（确定扫描开始点）
    }

    LimitL(IntervalLow);   //确定左扫描区间并进行限制
    LimitH(IntervalHigh);  //确定右扫描区间并进行限制
    GetJumpPointFromDet(PicTemp, 'R', IntervalLow, IntervalHigh,&JumpPoint[1]);     //扫右边线

    IntervalLow =ImageDeal[Ysite + 1].LeftBorder -ImageScanInterval;                //从上一行左边线-5的点开始（确定扫描开始点）
    IntervalHigh =ImageDeal[Ysite + 1].LeftBorder +ImageScanInterval;               //到上一行左边线+5的点结束（确定扫描结束点）

    LimitL(IntervalLow);   //确定左扫描区间并进行限制
    LimitH(IntervalHigh);  //确定右扫描区间并进行限制
    GetJumpPointFromDet(PicTemp, 'L', IntervalLow, IntervalHigh,&JumpPoint[0]);

    if (JumpPoint[0].type =='W')                                                    //如果本行左边线不正常跳变，即这10个点都是白的
    {
      ImageDeal[Ysite].LeftBorder =ImageDeal[Ysite + 1].LeftBorder;                 //本行左边线用上一行的数值
    } else                                                                          //左边线正常
    {
      ImageDeal[Ysite].LeftBorder = JumpPoint[0].point;                             //记录下来啦
    }

    if (JumpPoint[1].type == 'W')                                                   //如果本行右边线不正常跳变
    {
      ImageDeal[Ysite].RightBorder =ImageDeal[Ysite + 1].RightBorder;               //本行右边线用上一行的数值
    } else                                                                          //右边线正常
    {
      ImageDeal[Ysite].RightBorder = JumpPoint[1].point;                            //记录下来啦
    }

    ImageDeal[Ysite].IsLeftFind =JumpPoint[0].type;                                 //记录本行是否找到边线，即边线类型
    ImageDeal[Ysite].IsRightFind = JumpPoint[1].type;

    //重新确定那些大跳变的边缘
    if (( ImageDeal[Ysite].IsLeftFind == 'H'
         ||ImageDeal[Ysite].IsRightFind == 'H')) {
      if (ImageDeal[Ysite].IsLeftFind == 'H')                                   //如果左边线大跳变
        for (Xsite = (ImageDeal[Ysite].LeftBorder + 1);
             Xsite <= (ImageDeal[Ysite].RightBorder - 1);
             Xsite++)                                                           //左右边线之间重新扫描
        {
          if ((*(PicTemp + Xsite) == 0) && (*(PicTemp + Xsite + 1) != 0)) {
            ImageDeal[Ysite].LeftBorder =Xsite;                                 //如果上一行左边线的右边有黑白跳变则为绝对边线直接取出
            ImageDeal[Ysite].IsLeftFind = 'T';
            break;
          } else if (*(PicTemp + Xsite) != 0)                                   //一旦出现白点则直接跳出
            break;
          else if (Xsite ==(ImageDeal[Ysite].RightBorder - 1))
          {
             ImageDeal[Ysite].IsLeftFind = 'T';
            break;
          }
        }
      if ((ImageDeal[Ysite].RightBorder - ImageDeal[Ysite].LeftBorder) <=
          7)                              //图像宽度限定
      {
        ImageStatus.OFFLine = Ysite + 1;  //如果这行比7小了后面直接不要了
        break;
      }
      if (ImageDeal[Ysite].IsRightFind == 'H')
        for (Xsite = (ImageDeal[Ysite].RightBorder - 1);
             Xsite >= (ImageDeal[Ysite].LeftBorder + 1); Xsite--) {
          if ((*(PicTemp + Xsite) == 0) && (*(PicTemp + Xsite - 1) != 0)) {
            ImageDeal[Ysite].RightBorder =
                Xsite;                    //如果右边线的左边还有黑白跳变则为绝对边线直接取出
            ImageDeal[Ysite].IsRightFind = 'T';
            break;
          } else if (*(PicTemp + Xsite) != 0)
            break;
          else if (Xsite == (ImageDeal[Ysite].LeftBorder + 1))
          {
            ImageDeal[Ysite].RightBorder = Xsite;
            ImageDeal[Ysite].IsRightFind = 'T';
            break;
          }
        }
    }

 /***********重新确定无边行************/
    int ysite = 0;
    uint8_t L_found_point = 0;
    uint8_t R_found_point = 0;


    if( /*SystemData.SpeedData.Length * OX <SystemData.barn_lenth&&!!!!!!!*/
            ImageStatus.Road_type != Ramp){
    if (    ImageDeal[Ysite].IsRightFind == 'W'
          &&Ysite > 10
          &&Ysite < 50
          &&ImageStatus.Road_type!=Barn_in
          )                     //最早出现的无边行
    {
      if (Get_R_line == 'F')    //这一帧图像没有跑过这个找基准线的代码段才运行
      {
        Get_R_line = 'T';       //找了  一帧图像只跑一次 置为T
        ytemp_W_R = Ysite + 2;
        for (ysite = Ysite + 1; ysite < Ysite + 15; ysite++) {
          if (ImageDeal[ysite].IsRightFind =='T')  //往无边行下面搜索  一般都是有边的
            R_found_point++;
        }
        if (R_found_point >8)                      //找到基准斜率边  做延长线重新确定无边   当有边的点数大于8
        {
          D_R = ((float)(ImageDeal[Ysite + R_found_point].RightBorder - ImageDeal[Ysite + 3].RightBorder)) /((float)(R_found_point - 3));
                                                  //求下面这些点连起来的斜率
                                                  //好给无边行做延长线左个基准
          if (D_R > 0) {
            R_Found_T ='T';                       //如果斜率大于0  那么找到了这个基准行  因为梯形畸变
                                                  //所以一般情况都是斜率大于0  小于0的情况也不用延长 没必要
          } else {
            R_Found_T = 'F';                      //没有找到这个基准行
            if (D_R < 0)
              ExtenRFlag = 'F';                   //这个标志位用于十字角点补线  防止图像误补用的
          }
        }
      }
      if (R_Found_T == 'T')
        ImageDeal[Ysite].RightBorder =ImageDeal[ytemp_W_R].RightBorder -D_R * (ytemp_W_R - Ysite);  //如果找到了 那么以基准行做延长线

      LimitL(ImageDeal[Ysite].RightBorder);  //限幅
      LimitH(ImageDeal[Ysite].RightBorder);  //限幅
    }

    if (ImageDeal[Ysite].IsLeftFind == 'W' && Ysite > 10 && Ysite < 50 &&
        ImageStatus.Road_type != Barn_in)    //下面同理  左边界
    {
      if (Get_L_line == 'F') {
        Get_L_line = 'T';
        ytemp_W_L = Ysite + 2;
        for (ysite = Ysite + 1; ysite < Ysite + 15; ysite++) {
          if (ImageDeal[ysite].IsLeftFind == 'T')
            L_found_point++;
        }
        if (L_found_point > 8)              //找到基准斜率边  做延长线重新确定无边
        {
          D_L = ((float)(ImageDeal[Ysite + 3].LeftBorder -ImageDeal[Ysite + L_found_point].LeftBorder)) /((float)(L_found_point - 3));
          if (D_L > 0) {
            L_Found_T = 'T';

          } else {
            L_Found_T = 'F';
            if (D_L < 0)
              ExtenLFlag = 'F';
          }
        }
      }

      if (L_Found_T == 'T')
        ImageDeal[Ysite].LeftBorder =ImageDeal[ytemp_W_L].LeftBorder + D_L * (ytemp_W_L - Ysite);

      LimitL(ImageDeal[Ysite].LeftBorder);  //限幅
      LimitH(ImageDeal[Ysite].LeftBorder);  //限幅
    }
}

    if (   ImageDeal[Ysite].IsLeftFind == 'W'
         &&ImageDeal[Ysite].IsRightFind == 'W')
          {ImageStatus.WhiteLine++;}  //要是左右都无边，丢边数+1}
    if (ImageDeal[Ysite].IsLeftFind == 'W'&&Ysite<55)
    {
             ImageStatus.Left_Line++;
    }
    if (ImageDeal[Ysite].IsRightFind == 'W'&&Ysite<55)
    {
          ImageStatus.Right_Line++;
    }
      LimitL(ImageDeal[Ysite].LeftBorder);   //限幅
      LimitH(ImageDeal[Ysite].LeftBorder);   //限幅
      LimitL(ImageDeal[Ysite].RightBorder);  //限幅
      LimitH(ImageDeal[Ysite].RightBorder);  //限幅

      ImageDeal[Ysite].Wide =ImageDeal[Ysite].RightBorder - ImageDeal[Ysite].LeftBorder;
      ImageDeal[Ysite].Center =(ImageDeal[Ysite].RightBorder + ImageDeal[Ysite].LeftBorder) / 2;

    if (ImageDeal[Ysite].Wide <= 7) {
        ImageStatus.OFFLine = Ysite + 1;
        break;
    }
    else if (ImageDeal[Ysite].RightBorder <= 10 || ImageDeal[Ysite].LeftBorder >= 70) {
        ImageStatus.OFFLine = Ysite + 1;
        break;
    }                                        //当图像宽度小于0或者左右边达到一定的限制时，则终止巡边
  }


  return;
}

//延长线绘制，理论上来说是很准确的
void DrawExtensionLine() {
  if ((
        ImageStatus.Road_type != Barn_in
        &&ImageStatus.Road_type != Ramp)
//        &&ImageStatus.pansancha_Lenth* OX==0
        &&ImageStatus.Road_type !=LeftCirque
        &&ImageStatus.Road_type !=RightCirque
        )                                  // g5.22  6.22    ע     ǵøĻ   
  {
    if (ImageStatus.WhiteLine >= ImageStatus.TowPoint_True - 15)
      TFSite = 55;
//    if (ImageStatus.CirqueOff == 'T' && ImageStatus.Road_type == LeftCirque)
//      TFSite = 55;
    if (ExtenLFlag != 'F')
      for (Ysite = 54; Ysite >= (ImageStatus.OFFLine + 4);
           Ysite--)                       // ӵ    п ʼ    ɨɨ                   β   
                                          //      ֻ  һ  
      {
        PicTemp = Pixle[Ysite];           // 浱ǰ  
        if (ImageDeal[Ysite].IsLeftFind =='W')                          //         ߽ ûɨ    ɨ     ǰ ɫ  ˵      û    ߽  
        {
          //**************************************************//**************************************************
          if (ImageDeal[Ysite + 1].LeftBorder >= 70)                    //     ߽ ʵ    ̫ ұ 
          {
            ImageStatus.OFFLine = Ysite + 1;
            break;                        //ֱ                 
          }
          //************************************************//*************************************************

          while (Ysite >= (ImageStatus.OFFLine + 4))                    //  ʱ  ûɨ      
          {
            Ysite--;                      //        ɨ
            if (  ImageDeal[Ysite].IsLeftFind == 'T'
                &&ImageDeal[Ysite - 1].IsLeftFind == 'T'
                &&ImageDeal[Ysite - 2].IsLeftFind == 'T'
                &&ImageDeal[Ysite - 2].LeftBorder > 0
                &&ImageDeal[Ysite - 2].LeftBorder <70
                )                                                       //   ɨ     г    ˲  ұ              ж     ߽ 㣨  ߽  ڿհ  Ϸ   
            {
              FTSite = Ysite - 2;          // ѱ       ĵڶ  д   FTsite
              break;
            }
          }

          DetL =
              ((float)(ImageDeal[FTSite].LeftBorder -
                       ImageDeal[TFSite].LeftBorder)) /
              ((float)(FTSite - TFSite));  //  ߽  б ʣ  е      / е      
          if (FTSite > ImageStatus.OFFLine)
            for (
                ytemp = TFSite; ytemp >= FTSite; ytemp--)               // ӵ һ  ɨ      ߽      ڶ  е    꿪ʼ    ɨֱ   հ  Ϸ     ߽        ֵ
            {
              ImageDeal[ytemp].LeftBorder =
                  (int)(DetL * ((float)(ytemp - TFSite))) +
                  ImageDeal[TFSite]
                      .LeftBorder;                                      //     ڼ Ŀհ״    ߣ   б ߣ   Ŀ   Ƿ   ͼ    
            }
        } else
          TFSite = Ysite + 2;                                           //   ɨ   ˱  е   ߽磬   д        棬    б ʣ 
      }

    if (ImageStatus.WhiteLine >= ImageStatus.TowPoint_True - 15)
      TFSite = 55;
    // g5.22
    if (ImageStatus.CirqueOff == 'T' && ImageStatus.Road_type == RightCirque)
      TFSite = 55;
    if (ExtenRFlag != 'F')
      for (Ysite = 54; Ysite >= (ImageStatus.OFFLine + 4);
           Ysite--)               // ӵ    п ʼ    ɨɨ              
      {
        PicTemp = Pixle[Ysite];  // 浱ǰ  

        if (ImageDeal[Ysite].IsRightFind =='W')                       //        ұ߽ ûɨ    ɨ     ǰ ɫ  ˵      û   ұ߽ 㣬   Ǵ        ڵ 
        {
          if (ImageDeal[Ysite + 1].RightBorder <= 10)                 //    ұ߽ ʵ    ̫   
          {
            ImageStatus.OFFLine =Ysite + 1;                           //ֱ        ˵                       
            break;
          }
          while (Ysite >= (ImageStatus.OFFLine + 4))                  //  ʱ  ûɨ              
          {
            Ysite--;
            if (  ImageDeal[Ysite].IsRightFind == 'T'
                &&ImageDeal[Ysite - 1].IsRightFind == 'T'
                &&ImageDeal[Ysite - 2].IsRightFind == 'T'
                &&ImageDeal[Ysite - 2].RightBorder < 70
                &&ImageDeal[Ysite - 2].RightBorder > 10
                )                                                      //   ɨ     г    ˲  ұ              ж     ߽ 㣨  ߽  ڿհ  Ϸ   
            {
              FTSite = Ysite - 2;                                      //  ѱ       ĵڶ  д   FTsite
              break;
            }
          }

          DetR =((float)(ImageDeal[FTSite].RightBorder -ImageDeal[TFSite].RightBorder)) /((float)(FTSite - TFSite));         // ұ߽  б ʣ  е      / е      
          if (FTSite > ImageStatus.OFFLine)
            for (ytemp = TFSite; ytemp >= FTSite;ytemp--)              // ӵ һ  ɨ     ұ߽      ڶ  е    꿪ʼ    ɨֱ   հ  Ϸ    ұ߽        ֵ
            {
              ImageDeal[ytemp].RightBorder =(int)(DetR * ((float)(ytemp - TFSite))) +ImageDeal[TFSite].RightBorder;          //     ڼ Ŀհ״    ߣ   б ߣ   Ŀ   Ƿ   ͼ    
            }
        } else
          TFSite =Ysite +2;                                           //      е  ұ߽  ҵ  ˣ   Ѹ       ڶ        ͸ TFsite
      }
  }
  for (Ysite = 59; Ysite >= ImageStatus.OFFLine; Ysite--) {
    ImageDeal[Ysite].Center =(ImageDeal[Ysite].LeftBorder + ImageDeal[Ysite].RightBorder) /2;                                //ɨ           һ 龭 Ż ֮    м ֵ    
    ImageDeal[Ysite].Wide =-ImageDeal[Ysite].LeftBorder +ImageDeal[Ysite].RightBorder;                                       //   Ż ֮  Ŀ  ȴ   
  }
}

//---------------------------------------------------------------------------------------------------------------------------------------------------------------
//  @name           Search_Left_and_Right_Lines
//  @brief          通过sobel获取左右边界
//  @param          imageInput[IMAGE_ROW][IMAGE_COL]        输入的图像数据
//  @param          Row                                     图像行数
//  @param          Col                                     图像列数
//  @param          Bottonline                              底边线选择
//  @return         无
//  @time           2022年10月7日
//  @Author
//  Sample usage:   Search_Left_and_Right_Lines(imageInput, Row, Col, Bottonline);
//--------------------------------------------------------------------------------------------------------------------------------------------

void Search_Left_and_Right_Lines(uint8 imageInput[LCDH][LCDW], uint8 Row, uint8 Col, uint8 Bottonline)
{
    // 定义小车的当前搜索状态位置为 上 下 左 右 一共要搜 上：白色为黑色 左上边为白色 下：右边为色  右：左边有黑色
/*  前进方向定义：
                *   0
                * 3   1
                *   2
*/
/*寻找左边界规则*/
    int Left_Rule[2][8] = {
                                  {0,-1,1,0,0,1,-1,0 },//{0,-1},{1,0},{0,1},{-1,0},  (x,y)
                                  {-1,-1,1,-1,1,1,-1,1} //{-1,-1},{1,-1},{1,1},{-1,1}
    };
    /*寻找右边界规则*/
    int Right_Rule[2][8] = {
                              {0,-1,1,0,0,1,-1,0 },//{0,-1},{1,0},{0,1},{-1,0},
                              {1,-1,1,1,-1,1,-1,-1} //{1,-1},{1,1},{-1,1},{-1,-1}
    };
      int num=0;
    uint8 Left_Ysite = Bottonline;
    uint8 Left_Xsite = ImageDeal[Bottonline].LeftBoundary;
    uint8 Left_Rirection = 0;  // 左边方向
    uint8 Pixel_Left_Ysite = Bottonline;
    uint8 Pixel_Left_Xsite = 0;

    uint8 Right_Ysite = Bottonline;
    uint8 Right_Xsite = ImageDeal[Bottonline].RightBoundary;
    uint8 Right_Rirection = 0;  // 右边方向
    uint8 Pixel_Right_Ysite = Bottonline;
    uint8 Pixel_Right_Xsite = 0;
    uint8 Ysite = Bottonline;
    ImageStatus.OFFLineBoundary = 5;
    while (1)
    {
            num++;
            if(num>400)
            {
                 ImageStatus.OFFLineBoundary = Ysite;
                break;
            }
        if (Ysite >= Pixel_Left_Ysite && Ysite >= Pixel_Right_Ysite)
        {
            if (Ysite < ImageStatus.OFFLineBoundary)
            {
                ImageStatus.OFFLineBoundary = Ysite;
                break;
            }
            else
            {
                Ysite--;
            }
        }
        /*********左边巡线*******/
        if ((Pixel_Left_Ysite > Ysite) || Ysite == ImageStatus.OFFLineBoundary)  // 右边扫描
        {
            /*检测前方像素*/
            Pixel_Left_Ysite = Left_Ysite + Left_Rule[0][2 * Left_Rirection + 1];
            Pixel_Left_Xsite = Left_Xsite + Left_Rule[0][2 * Left_Rirection];

            if (imageInput[Pixel_Left_Ysite][Pixel_Left_Xsite] == 0)  // 前方是黑色
            {
                // 顺时针旋转90度
                if (Left_Rirection == 3)
                    Left_Rirection = 0;
                else
                    Left_Rirection++;
            }
            else  // 前方是白色
            {
                /*检测斜前方像素*/
                Pixel_Left_Ysite = Left_Ysite + Left_Rule[1][2 * Left_Rirection + 1];
                Pixel_Left_Xsite = Left_Xsite + Left_Rule[1][2 * Left_Rirection];

                if (imageInput[Pixel_Left_Ysite][Pixel_Left_Xsite] == 0)  // 斜前方为黑色
                {
                    // 方向不变
                    Left_Ysite = Left_Ysite + Left_Rule[0][2 * Left_Rirection + 1];
                    Left_Xsite = Left_Xsite + Left_Rule[0][2 * Left_Rirection];
                    if (ImageDeal[Left_Ysite].LeftBoundary_First == 0){
                        ImageDeal[Left_Ysite].LeftBoundary_First = Left_Xsite;
                        ImageDeal[Left_Ysite].LeftBoundary = Left_Xsite;
                    }
                }
                else  // 斜前方为白色
                {
                    // 方向改变，逆时针90度
                    Left_Ysite = Left_Ysite + Left_Rule[1][2 * Left_Rirection + 1];
                    Left_Xsite = Left_Xsite + Left_Rule[1][2 * Left_Rirection];
                    if (ImageDeal[Left_Ysite].LeftBoundary_First == 0 )
                        ImageDeal[Left_Ysite].LeftBoundary_First = Left_Xsite;
                    ImageDeal[Left_Ysite].LeftBoundary = Left_Xsite;
                    if (Left_Rirection == 0)
                        Left_Rirection = 3;
                    else
                        Left_Rirection--;
                }
            }
        }
        /*********右边巡线*******/
        if ((Pixel_Right_Ysite > Ysite) || Ysite == ImageStatus.OFFLineBoundary)  // 右边扫描
        {
            /*检测前方像素*/
            Pixel_Right_Ysite = Right_Ysite + Right_Rule[0][2 * Right_Rirection + 1];
            Pixel_Right_Xsite = Right_Xsite + Right_Rule[0][2 * Right_Rirection];

            if (imageInput[Pixel_Right_Ysite][Pixel_Right_Xsite] == 0)  // 前方是黑色
            {
                // 逆时针旋转90度
                if (Right_Rirection == 0)
                    Right_Rirection = 3;
                else
                    Right_Rirection--;
            }
            else  // 前方是白色
            {
                /*检测斜前方像素*/
                Pixel_Right_Ysite = Right_Ysite + Right_Rule[1][2 * Right_Rirection + 1];
                Pixel_Right_Xsite = Right_Xsite + Right_Rule[1][2 * Right_Rirection];

                if (imageInput[Pixel_Right_Ysite][Pixel_Right_Xsite] == 0)  // 斜前方为黑色
                {
                    // 方向不变
                    Right_Ysite = Right_Ysite + Right_Rule[0][2 * Right_Rirection + 1];
                    Right_Xsite = Right_Xsite + Right_Rule[0][2 * Right_Rirection];
                    if (ImageDeal[Right_Ysite].RightBoundary_First == 79 )
                        ImageDeal[Right_Ysite].RightBoundary_First = Right_Xsite;
                    ImageDeal[Right_Ysite].RightBoundary = Right_Xsite;
                }
                else  // 斜前方为白色
                {
                    // 方向改变，顺时针90度
                    Right_Ysite = Right_Ysite + Right_Rule[1][2 * Right_Rirection + 1];
                    Right_Xsite = Right_Xsite + Right_Rule[1][2 * Right_Rirection];
                    if (ImageDeal[Right_Ysite].RightBoundary_First == 79)
                        ImageDeal[Right_Ysite].RightBoundary_First = Right_Xsite;
                    ImageDeal[Right_Ysite].RightBoundary = Right_Xsite;
                    if (Right_Rirection == 3)
                        Right_Rirection = 0;
                    else
                        Right_Rirection++;
                }
            }
        }

        if (abs(Pixel_Right_Xsite - Pixel_Left_Xsite) < 3)  // 为了防止底部是白点而扫描错误
        {
            ImageStatus.OFFLineBoundary = Ysite;
            break;
        }
    }
}

/*ϽַɨߣΪԲжԪصĵڶ*/
//---------------------------------------------------------------------------------------------------------------------------------------------------------------
//  @name           Search_Bottom_Line_OTSU
//  @brief          获取底部左右边界
//  @param          imageInput[IMAGE_ROW][IMAGE_COL]        输入图像数据
//  @param          Row                                     图像行数
//  @param          Col                                     图像列数
//  @return         Bottonline                             底边线选择
//  @time           2022年10月9日
//  @Author
//  Sample usage:   Search_Bottom_Line_OTSU(imageInput, Row, Col, Bottonline);
//--------------------------------------------------------------------------------------------------------------------------------------------

void Search_Bottom_Line_OTSU(uint8 imageInput[LCDH][LCDW], uint8 Row, uint8 Col, uint8 Bottonline)
{
    // 寻找左右边界
    for (int Xsite = Col / 2-2; Xsite > 1; Xsite--)
    {
        if (imageInput[Bottonline][Xsite] == 1 && imageInput[Bottonline][Xsite - 1] == 0)
        {
            ImageDeal[Bottonline].LeftBoundary = Xsite;  // 获取底边左边界
            break;
        }
    }
    for (int Xsite = Col / 2+2; Xsite < LCDW-1; Xsite++)
    {
        if (imageInput[Bottonline][Xsite] == 1 && imageInput[Bottonline][Xsite + 1] == 0)
        {
            ImageDeal[Bottonline].RightBoundary = Xsite;  // 获取底边右边界
            break;
        }
    }
}

void Search_Border_OTSU(uint8 imageInput[LCDH][LCDW], uint8 Row, uint8 Col, uint8 Bottonline)
{
    ImageStatus.WhiteLine_L = 0;
    ImageStatus.WhiteLine_R = 0;
    
    // 清除上下边界
    for (int Xsite = 0; Xsite < LCDW; Xsite++)
    {
        imageInput[0][Xsite] = 0;
        imageInput[Bottonline + 1][Xsite] = 0;
    }
    
    // 清除左右边界
    for (int Ysite = 0; Ysite < LCDH; Ysite++)
    {
        ImageDeal[Ysite].LeftBoundary_First = 0;
        ImageDeal[Ysite].RightBoundary_First = 79;

        imageInput[Ysite][0] = 0;
        imageInput[Ysite][LCDW - 1] = 0;
    }
    
    // 获取底线
    Search_Bottom_Line_OTSU(imageInput, Row, Col, Bottonline);
    
    // 获取左右边界
    Search_Left_and_Right_Lines(imageInput, Row, Col, Bottonline);

    // 统计丢线情况
    for (int Ysite = Bottonline; Ysite > ImageStatus.OFFLineBoundary + 1; Ysite--)
    {
        if (ImageDeal[Ysite].LeftBoundary < 3)
        {
            ImageStatus.WhiteLine_L++;  // 左边丢线计数
        }
        if (ImageDeal[Ysite].RightBoundary > LCDW - 3)
        {
            ImageStatus.WhiteLine_R++;  // 右边丢线计数
        }
    }
}

// 路径滤波函数
// 当出现丢边的情况时，重新计算无边行的中线位置
void RouteFilter() {
    for (Ysite = 58; Ysite >= (ImageStatus.OFFLine + 5); Ysite--) {
        // 检测连续两行都丢失左右边界的情况
        if (ImageDeal[Ysite].IsLeftFind == 'W' &&
            ImageDeal[Ysite].IsRightFind == 'W' &&
            Ysite <= 45 &&
            ImageDeal[Ysite - 1].IsLeftFind == 'W' &&
            ImageDeal[Ysite - 1].IsRightFind == 'W') {
            
            // 向上搜索直到找到有效的边界点
            ytemp = Ysite;
            while (ytemp >= (ImageStatus.OFFLine + 5)) {
                ytemp--;
                // 找到同时具有左右边界的行
                if (ImageDeal[ytemp].IsLeftFind == 'T' &&
                    ImageDeal[ytemp].IsRightFind == 'T') {
                    
                    // 计算中线斜率
                    DetR = (float)(ImageDeal[ytemp - 1].Center - ImageDeal[Ysite + 2].Center) /
                           (float)(ytemp - 1 - Ysite - 2);
                    
                    // 记录初始位置信息
                    int CenterTemp = ImageDeal[Ysite + 2].Center;
                    int LineTemp = Ysite + 2;
                    
                    // 使用线性插值补全中线
                    while (Ysite >= ytemp) {
                        ImageDeal[Ysite].Center = (int)(CenterTemp + DetR * (float)(Ysite - LineTemp));
                        Ysite--;
                    }
                    break;
                }
            }
        }
        // 使用加权平均平滑中线
        ImageDeal[Ysite].Center = (ImageDeal[Ysite - 1].Center + 2 * ImageDeal[Ysite].Center) / 3;
    }
}

//--------------------------------------------------------------
//  @name           Element_Judgment_Left_Rings()
//  @brief          左圆环判断函数，用于判断是否进入左圆环
//  @parameter      void
//  @time
//  @Author         MRCHEN
//  Sample usage:   Element_Judgment_Left_Rings();
//-------------------------------------------------------------
void Element_Judgment_Left_Rings()
{
//    Disf = 0;
    if (
              ImageStatus.Right_Line > 10
            ||ImageStatus.Left_Line < 12 // 13
            ||ImageStatus.OFFLine > 10
            || Straight_Judge(2) > 1
            || ImageStatus.WhiteLine>10
//            || (ImageDeal[48].RightBorder - ImageDeal[48].LeftBorder)<51
            || ImageDeal[52].IsLeftFind == 'W'
            || ImageDeal[53].IsLeftFind == 'W'
            || ImageDeal[54].IsLeftFind == 'W'
            || ImageDeal[55].IsLeftFind == 'W'
            || ImageDeal[56].IsLeftFind == 'W'
            || ImageDeal[57].IsLeftFind == 'W'
            || ImageDeal[58].IsLeftFind == 'W'
        )
            return;
    int ring_ysite = 25;
  //  uint8 Left_Less_Num = 0;
    Left_RingsFlag_Point1_Ysite = 0;
    Left_RingsFlag_Point2_Ysite = 0;
 //   ceshi_flag = 1;
    for (int Ysite = 58; Ysite > ring_ysite; Ysite--)
    {
        if (ImageDeal[Ysite].LeftBoundary_First - ImageDeal[Ysite - 1].LeftBoundary_First > 4)
        {
            Left_RingsFlag_Point1_Ysite = Ysite;
            break;
        }
    }
    for (int Ysite = 58; Ysite > ring_ysite; Ysite--)
    {
        if (ImageDeal[Ysite + 1].LeftBoundary - ImageDeal[Ysite].LeftBoundary > 4)
        {
            Left_RingsFlag_Point2_Ysite = Ysite;
            break;
        }
    }

    for (int Ysite = Left_RingsFlag_Point1_Ysite; Ysite > ImageStatus.OFFLine; Ysite--)
    {
//        if (ImageDeal[Ysite + 3].LeftBoundary_First < ImageDeal[Ysite].LeftBoundary_First
//            && ImageDeal[Ysite + 2].LeftBoundary_First < ImageDeal[Ysite].LeftBoundary_First
//            && ImageDeal[Ysite].LeftBoundary_First > ImageDeal[Ysite - 1].LeftBoundary_First
//            && ImageDeal[Ysite].LeftBoundary_First > ImageDeal[Ysite - 1].LeftBoundary_First
//            )
        if (   ImageDeal[Ysite + 6].LeftBorder < ImageDeal[Ysite+3].LeftBorder
            && ImageDeal[Ysite + 5].LeftBorder < ImageDeal[Ysite+3].LeftBorder
            && ImageDeal[Ysite + 3].LeftBorder > ImageDeal[Ysite + 2].LeftBorder
            && ImageDeal[Ysite + 3].LeftBorder > ImageDeal[Ysite + 1].LeftBorder
            )
        {
            Ring_Help_Flag = 1;
            break;
        }
    }
    if(Left_RingsFlag_Point2_Ysite > Left_RingsFlag_Point1_Ysite+1 && Ring_Help_Flag == 0 )
    {
        if(ImageStatus.Left_Line > 7)//13
            Ring_Help_Flag = 1;
    }
    if (Left_RingsFlag_Point2_Ysite > Left_RingsFlag_Point1_Ysite+1 && Ring_Help_Flag == 1 && ImageFlag.image_element_rings_flag ==0)
    {

        ImageFlag.image_element_rings = 1;
        ImageFlag.image_element_rings_flag = 1;
        ImageFlag.ring_big_small=1;

        ImageStatus.Road_type = LeftCirque;
        //gpio_set_level(P20_8, 0);
        //wireless_uart_send_byte(9);
    }
    Ring_Help_Flag = 0;
}

//--------------------------------------------------------------
//  @name           Element_Judgment_Right_Rings()
//  @brief          右圆环判断函数，用于判断是否进入右圆环
//  @parameter      void
//  @time
//  @Author         MRCHEN
//  Sample usage:   Element_Judgment_Right_Rings();
//--------------------------------------------------------------
void Element_Judgment_Right_Rings()
{
    if (   ImageStatus.Left_Line > 8
        || ImageStatus.Right_Line < 12 //13
        || ImageStatus.OFFLine >8
        || Straight_Judge(1) > 1
        || ImageStatus.WhiteLine>10
//        || (ImageDeal[48].RightBorder - ImageDeal[48].LeftBorder)<51
//        || (ImageDeal[18].RightBoundary_First - ImageDeal[18].LeftBoundary_First)<70
        || ImageDeal[52].IsRightFind == 'W'
        || ImageDeal[53].IsRightFind == 'W'
        || ImageDeal[54].IsRightFind == 'W'
        || ImageDeal[55].IsRightFind == 'W'
        || ImageDeal[56].IsRightFind == 'W'
        || ImageDeal[57].IsRightFind == 'W'
        || ImageDeal[58].IsRightFind == 'W'
        )
        {return;}
    int ring_ysite = 25;
    Right_RingsFlag_Point1_Ysite = 0;
    Right_RingsFlag_Point2_Ysite = 0;
    for (int Ysite = 58; Ysite > ring_ysite; Ysite--)
    {
        if (ImageDeal[Ysite - 1].RightBoundary_First - ImageDeal[Ysite].RightBoundary_First > 4)
        {
            Right_RingsFlag_Point1_Ysite = Ysite;
            break;
        }
    }
    for (int Ysite = 58; Ysite > ring_ysite; Ysite--)
    {
        if (ImageDeal[Ysite].RightBoundary - ImageDeal[Ysite + 1].RightBoundary > 4)
        {
            Right_RingsFlag_Point2_Ysite = Ysite;
            break;
        }
    }
    for (int Ysite = Right_RingsFlag_Point1_Ysite; Ysite > 10; Ysite--)
    {
        if (   ImageDeal[Ysite + 6].RightBorder > ImageDeal[Ysite + 3].RightBorder
            && ImageDeal[Ysite + 5].RightBorder > ImageDeal[Ysite + 3].RightBorder
            && ImageDeal[Ysite + 3].RightBorder < ImageDeal[Ysite + 2].RightBorder
            && ImageDeal[Ysite + 3].RightBorder < ImageDeal[Ysite + 1].RightBorder
           )
        {
            Ring_Help_Flag = 1;
            break;
        }
    }
    if(Right_RingsFlag_Point2_Ysite > Right_RingsFlag_Point1_Ysite+1 && Ring_Help_Flag == 0)
    {
        if(ImageStatus.Right_Line>7)
            Ring_Help_Flag = 1;
    }
    if (Right_RingsFlag_Point2_Ysite > Right_RingsFlag_Point1_Ysite+1 && Ring_Help_Flag == 1 && ImageFlag.image_element_rings_flag == 0)
    {

        ImageFlag.image_element_rings = 2;
        ImageFlag.image_element_rings_flag = 1;
        ImageFlag.ring_big_small=1;     //С  
        ImageStatus.Road_type = RightCirque;
//        flag_ceshi++;
//        gpio_set_level(Bee1p, 1);
    }
    Ring_Help_Flag = 0;
}

//--------------------------------------------------------------
//  @name           Element_Handle_Left_Rings()
//  @brief          左圆环处理函数，处理左圆环的识别和路径规划
//  @parameter      void
//  @time
//  @Author         MRCHEN
//  Sample usage:   Element_Handle_Left_Rings();
//-------------------------------------------------------------
void Element_Handle_Left_Rings()
{
    /***************************************判断**************************************/
    int num = 0;
    for (int Ysite = 55; Ysite > 30; Ysite--)
    {
        if(ImageDeal[Ysite].IsLeftFind == 'W')
            num++;
        if(    ImageDeal[Ysite+3].IsLeftFind == 'W' && ImageDeal[Ysite+2].IsLeftFind == 'W'
            && ImageDeal[Ysite+1].IsLeftFind == 'W' && ImageDeal[Ysite].IsLeftFind == 'T')
            break;
    }

    // 准备进环
    if (ImageFlag.image_element_rings_flag == 1 && num>10)
    {
        ImageFlag.image_element_rings_flag = 2;
        //wireless_uart_send_byte(2);
    }
    if (ImageFlag.image_element_rings_flag == 2 && num<8)
    {
        ImageFlag.image_element_rings_flag = 5;
        //wireless_uart_send_byte(5);
    }
    // 进环
    if(ImageFlag.image_element_rings_flag == 5 && /*num>15)*/ImageStatus.Right_Line>15)
    {
        ImageFlag.image_element_rings_flag = 6;
        //wireless_uart_send_byte(6);
    }
    // 进入小圆环
    if(ImageFlag.image_element_rings_flag == 6 && ImageStatus.Right_Line<4)
    {
        ImageFlag.image_element_rings_flag = 7;
        //wireless_uart_send_byte(8);
    }
    // 进入 大圆环判断
    if (ImageFlag.ring_big_small == 1 && ImageFlag.image_element_rings_flag == 7)
    {
        Point_Ysite = 0;
        Point_Xsite = 0;
        for (int Ysite = 50; Ysite > ImageStatus.OFFLine + 3; Ysite--)
        {
            if (       ImageDeal[Ysite].RightBorder <= ImageDeal[Ysite + 2].RightBorder
                    && ImageDeal[Ysite].RightBorder <= ImageDeal[Ysite - 2].RightBorder
                    && ImageDeal[Ysite].RightBorder <= ImageDeal[Ysite + 1].RightBorder
                    && ImageDeal[Ysite].RightBorder <= ImageDeal[Ysite - 1].RightBorder
                    && ImageDeal[Ysite].RightBorder <= ImageDeal[Ysite + 3].RightBorder
                    && ImageDeal[Ysite].RightBorder <= ImageDeal[Ysite - 3].RightBorder
               )
            {
                Point_Xsite = ImageDeal[Ysite].RightBorder;
                Point_Ysite = Ysite;
                break;
            }
        }
        if (Point_Ysite > 20)
        {
            ImageFlag.image_element_rings_flag = 8;
        }
    }

    // 进入 小圆环判断
    if (ImageFlag.image_element_rings_flag == 8)
    {
         if (ImageStatus.Right_Line < 7     // 左边在环道中工作正常，可以圆环进入下一状态，加强左边界判断
             && ImageStatus.OFFLine < 6)    // 右边为直道且截止行，前瞻值都很小
         {
             ImageFlag.image_element_rings_flag = 9;
         }
    }

    // 检测圆环出口
    if (ImageFlag.image_element_rings_flag == 9)
    {
        int num=0;
        for (int Ysite = 45; Ysite > 8; Ysite--)
        {
            if(ImageDeal[Ysite].IsLeftFind == 'W' )
                num++;
        }
        if(num < 5)
        {
            ImageStatus.Road_type = Normol;   // 重置道路类型为无特征
            ImageFlag.image_element_rings_flag = 0;
            ImageFlag.image_element_rings = 0;
            ImageFlag.ring_big_small = 0;
        }
    }

    /***************************************处理**************************************/
    // 准备进环 右线补线
    if (   ImageFlag.image_element_rings_flag == 1
        || ImageFlag.image_element_rings_flag == 2
        || ImageFlag.image_element_rings_flag == 3
        || ImageFlag.image_element_rings_flag == 4)
    {
        for (int Ysite = 57; Ysite > ImageStatus.OFFLine; Ysite--)
        {
            ImageDeal[Ysite].Center = ImageDeal[Ysite].RightBorder - Half_Road_Wide[Ysite]-5;
        }
    }
    // 准备 右线补线
    if  ( ImageFlag.image_element_rings_flag == 5
        ||ImageFlag.image_element_rings_flag == 6
        )
    {
        int  flag_Xsite_1=0;
        int flag_Ysite_1=0;
        float Slope_Rings=0;
        for(Ysite=55;Ysite>ImageStatus.OFFLine;Ysite--)  // 满足条件
        {
            for(Xsite=ImageDeal[Ysite].LeftBorder + 1;Xsite<ImageDeal[Ysite].RightBorder - 1;Xsite++)
            {
                if(  Pixle[Ysite][Xsite] == 1 && Pixle[Ysite][Xsite + 1] == 0)
                 {
                   flag_Ysite_1 = Ysite;
                   flag_Xsite_1 = Xsite;
                   Slope_Rings=(float)(79-flag_Xsite_1)/(float)(59-flag_Ysite_1);
                   break;
                 }
            }
            if(flag_Ysite_1 != 0)
            {
                break;
            }
        }
        if(flag_Ysite_1 == 0)
        {
            for(Ysite=ImageStatus.OFFLine+1;Ysite<30;Ysite++)
            {
                if(ImageDeal[Ysite].IsLeftFind=='T'&&ImageDeal[Ysite+1].IsLeftFind=='T'&&ImageDeal[Ysite+2].IsLeftFind=='W'
                    &&abs(ImageDeal[Ysite].LeftBorder-ImageDeal[Ysite+2].LeftBorder)>10
                  )
                {
                    flag_Ysite_1=Ysite;
                    flag_Xsite_1=ImageDeal[flag_Ysite_1].LeftBorder;
                    ImageStatus.OFFLine=Ysite;
                    Slope_Rings=(float)(79-flag_Xsite_1)/(float)(59-flag_Ysite_1);
                    break;
                }
            }
        }
        // 准备补线
        if(flag_Ysite_1 != 0)
        {
            for(Ysite=flag_Ysite_1;Ysite<60;Ysite++)
            {
                ImageDeal[Ysite].RightBorder=flag_Xsite_1+Slope_Rings*(Ysite-flag_Ysite_1);
                ImageDeal[Ysite].Center = ((ImageDeal[Ysite].RightBorder + ImageDeal[Ysite].LeftBorder)/2);
                if(ImageDeal[Ysite].Center<4)
                    ImageDeal[Ysite].Center = 4;
            }
            ImageDeal[flag_Ysite_1].RightBorder=flag_Xsite_1;
            for(Ysite=flag_Ysite_1-1;Ysite>10;Ysite--) // A边
            {
                for(Xsite=ImageDeal[Ysite+1].RightBorder-10;Xsite<ImageDeal[Ysite+1].RightBorder+2;Xsite++)
                {
                    if(Pixle[Ysite][Xsite]==1 && Pixle[Ysite][Xsite+1]==0)
                    {
                        ImageDeal[Ysite].RightBorder=Xsite;
                        ImageDeal[Ysite].Center = ((ImageDeal[Ysite].RightBorder + ImageDeal[Ysite].LeftBorder)/2);
                        if(ImageDeal[Ysite].Center<4)
                            ImageDeal[Ysite].Center = 4;
                        ImageDeal[Ysite].Wide=ImageDeal[Ysite].RightBorder-ImageDeal[Ysite].LeftBorder;
                        break;
                    }
                }

                if(ImageDeal[Ysite].Wide>8 &&ImageDeal[Ysite].RightBorder< ImageDeal[Ysite+2].RightBorder)
                {
                    continue;
                }
                else
                {
                    ImageStatus.OFFLine=Ysite+2;
                    break;
                }
            }
        }
    }
    // 小圆环处理
    if (ImageFlag.image_element_rings_flag == 7)
    {
        // 小圆环处理逻辑
    }
    // 大圆环处理
    if (ImageFlag.image_element_rings_flag == 8 && ImageFlag.ring_big_small == 1)    // 圆环
    {
        Repair_Point_Ysite = 7;

        for (int Ysite = 57; Ysite > Repair_Point_Ysite-3; Ysite--)         
        {
            ImageDeal[Ysite].RightBorder = ImageDeal[Ysite].LeftBorder + Half_Bend_Wide[Ysite];
            if(ImageDeal[Ysite].RightBorder>77){ImageDeal[Ysite].RightBorder = 77;}
            ImageDeal[Ysite].Center = ((ImageDeal[Ysite].RightBorder + ImageDeal[Ysite].LeftBorder) / 2);
        }
    }
    // 准备进环 右线补线
    if (ImageFlag.image_element_rings_flag == 9 || ImageFlag.image_element_rings_flag == 10)
    {
        for (int Ysite = 59; Ysite > ImageStatus.OFFLine; Ysite--)
        {
            ImageDeal[Ysite].Center = ImageDeal[Ysite].RightBorder - Half_Road_Wide[Ysite];
        }
    }
}

//--------------------------------------------------------------
//  @name           Element_Handle_Right_Rings()
//  @brief          右圆环处理函数，处理右圆环的识别和路径规划
//  @parameter      void
//  @time
//  @Author         MRCHEN
//  Sample usage:   Element_Handle_Right_Rings();
//-------------------------------------------------------------
void Element_Handle_Right_Rings()
{
    /****************判断*****************/
    int num =0 ;
    for (int Ysite = 55; Ysite > 30; Ysite--)
    {
        if(ImageDeal[Ysite].IsRightFind == 'W')
        {
            num++;
        }
        if(    ImageDeal[Ysite+3].IsRightFind == 'W' && ImageDeal[Ysite+2].IsRightFind == 'W'
            && ImageDeal[Ysite+1].IsRightFind == 'W' && ImageDeal[Ysite].IsRightFind == 'T' )
            break;
    }
        //准备进环  左线补线
    if (ImageFlag.image_element_rings_flag == 1 && num>10)
    {
        ImageFlag.image_element_rings_flag = 2;
    }
    if (ImageFlag.image_element_rings_flag == 2 && num<8)
    {
        ImageFlag.image_element_rings_flag = 5;
    }
        //准备
    if(ImageFlag.image_element_rings_flag == 5 && ImageStatus.Left_Line>15)
    {
        ImageFlag.image_element_rings_flag = 6;
       // ImageStatus.Road_type = RightCirque;
    }
        //圆
    if(ImageFlag.image_element_rings_flag == 6 && ImageStatus.Left_Line<4)
    {
        ImageFlag.image_element_rings_flag = 7;
        //Stop=1;
    }
    if (ImageFlag.image_element_rings_flag == 7)
    {
        Point_Xsite = 0;
        Point_Ysite = 0;
        for (int Ysite = 55; Ysite > ImageStatus.OFFLine+3; Ysite--)
        {
            if (    ImageDeal[Ysite].LeftBorder >= ImageDeal[Ysite + 2].LeftBorder
                 && ImageDeal[Ysite].LeftBorder >= ImageDeal[Ysite - 2].LeftBorder
                 && ImageDeal[Ysite].LeftBorder >= ImageDeal[Ysite + 1].LeftBorder
                 && ImageDeal[Ysite].LeftBorder >= ImageDeal[Ysite - 1].LeftBorder
                 && ImageDeal[Ysite].LeftBorder >= ImageDeal[Ysite + 4].LeftBorder
                 && ImageDeal[Ysite].LeftBorder >= ImageDeal[Ysite - 4].LeftBorder
                )

            {
                        Point_Xsite = ImageDeal[Ysite].LeftBorder;
                        Point_Ysite = Ysite;
                        break;
            }
        }
        if (Point_Ysite > 18)
        {
            ImageFlag.image_element_rings_flag = 8;
        }
        else if(ImageDeal[18].RightBoundary_First - ImageDeal[18].LeftBoundary_First>70)
        {
            ImageFlag.image_element_rings_flag = 8;
        }
    }
    if (ImageFlag.image_element_rings_flag == 8)
    {
         if (   //Straight_Judge(1, ImageStatus.OFFLine+10, 45) < 1
             /*&&*/ ImageStatus.Left_Line < 5 // 左边在环道中工作正常，可以圆环进入下一状态，例如，加强左边界的判断
             && ImageStatus.OFFLine < 8)    // 右边为直道且截止行，前瞻值都很小
            {ImageFlag.image_element_rings_flag = 9;}

    }
    if(ImageFlag.image_element_rings_flag == 9 )
    {
        int num=0;
        for (int Ysite = 45; Ysite > 10; Ysite--)
        {
            if(ImageDeal[Ysite].IsRightFind == 'W' )
            {
                num++;
            }
        }
        if(num < 5)
        {
            ImageStatus.Road_type = Normol;   // 重置道路类型为无特征
            ImageFlag.image_element_rings_flag = 0;
            ImageFlag.image_element_rings = 0;
            ImageFlag.ring_big_small = 0;
        }
    }
    /***************************************处理**************************************/
         //准备进环  左线补线
    if (   ImageFlag.image_element_rings_flag == 1
        || ImageFlag.image_element_rings_flag == 2
        || ImageFlag.image_element_rings_flag == 3
        || ImageFlag.image_element_rings_flag == 4)
    {
        for (int Ysite = 59; Ysite > ImageStatus.OFFLine; Ysite--)
        {
            ImageDeal[Ysite].Center = ImageDeal[Ysite].LeftBorder + Half_Road_Wide[Ysite];
        }
    }

        //准备  左线补线
    if (   ImageFlag.image_element_rings_flag == 5
        || ImageFlag.image_element_rings_flag == 6
       )
    {
        int flag_Xsite_1=0;
        int  flag_Ysite_1=0;
        float Slope_Right_Rings = 0;
        for(Ysite=55;Ysite>ImageStatus.OFFLine;Ysite--)
        {
            for(Xsite=ImageDeal[Ysite].LeftBorder + 1;Xsite<ImageDeal[Ysite].RightBorder - 1;Xsite++)
            {
                if(Pixle[Ysite][Xsite]==1 && Pixle[Ysite][Xsite+1]==0)
                {
                    flag_Ysite_1=Ysite;
                    flag_Xsite_1=Xsite;
                    Slope_Right_Rings=(float)(0-flag_Xsite_1)/(float)(59-flag_Ysite_1);
                    break;
                }
            }
            if(flag_Ysite_1!=0)
            {
              break;
            }
        }
        if(flag_Ysite_1==0)
        {
        for(Ysite=ImageStatus.OFFLine+5;Ysite<30;Ysite++)
        {
         if(ImageDeal[Ysite].IsRightFind=='T'&&ImageDeal[Ysite+1].IsRightFind=='T'&&ImageDeal[Ysite+2].IsRightFind=='W'
               &&abs(ImageDeal[Ysite].RightBorder-ImageDeal[Ysite+2].RightBorder)>10
         )
         {
             flag_Ysite_1=Ysite;
             flag_Xsite_1=ImageDeal[flag_Ysite_1].RightBorder;
             ImageStatus.OFFLine=Ysite;
             Slope_Right_Rings=(float)(0-flag_Xsite_1)/(float)(59-flag_Ysite_1);
             break;
         }

        }

        }
        //准备
        if(flag_Ysite_1!=0)
        {
            for(Ysite=flag_Ysite_1;Ysite<58;Ysite++)
            {
                ImageDeal[Ysite].LeftBorder=flag_Xsite_1+Slope_Right_Rings*(Ysite-flag_Ysite_1);

                    ImageDeal[Ysite].Center=(ImageDeal[Ysite].LeftBorder+ImageDeal[Ysite].RightBorder)/2;//
                if(ImageDeal[Ysite].Center>79)
                    ImageDeal[Ysite].Center=79;
            }
            ImageDeal[flag_Ysite_1].LeftBorder=flag_Xsite_1;
            for(Ysite=flag_Ysite_1-1;Ysite>10;Ysite--) //A边
            {
                for(Xsite=ImageDeal[Ysite+1].LeftBorder+8;Xsite>ImageDeal[Ysite+1].LeftBorder-4;Xsite--)
                {
                    if(Pixle[Ysite][Xsite]==1 && Pixle[Ysite][Xsite-1]==0)
                    {
                     ImageDeal[Ysite].LeftBorder=Xsite;
                     ImageDeal[Ysite].Wide=ImageDeal[Ysite].RightBorder-ImageDeal[Ysite].LeftBorder;
//                     if(ImageFlag.ring_big_small==2)//小圆
//                         ImageDeal[Ysite].Center=ImageDeal[Ysite].LeftBorder+Half_Bend_Wide[Ysite];//
//                     else//圆
                         ImageDeal[Ysite].Center=(ImageDeal[Ysite].LeftBorder+ImageDeal[Ysite].RightBorder)/2;//
                     if(ImageDeal[Ysite].Center>79)
                         ImageDeal[Ysite].Center=79;
                     if(ImageDeal[Ysite].Center<5)
                         ImageDeal[Ysite].Center=5;
                     break;
                    }
                }
                if(ImageDeal[Ysite].Wide>8 && ImageDeal[Ysite].LeftBorder>  ImageDeal[Ysite+2].LeftBorder)
                {
                    continue;
                }
                else
                {
                    ImageStatus.OFFLine=Ysite+2;
                    break;
                }
            }
        }
    }
        //圆
    if (ImageFlag.image_element_rings_flag == 7)
    {

    }
        //圆
    if (ImageFlag.image_element_rings_flag == 8)  //圆
    {
//        Repair_Point_Xsite = 42;
        Repair_Point_Ysite = 7;

        for (int Ysite = 57; Ysite > Repair_Point_Ysite-3; Ysite--)         //
        {
//            ImageDeal[Ysite].LeftBorder = (ImageDeal[58].LeftBorder - Repair_Point_Xsite) * (Ysite - 58) / (58 - Repair_Point_Ysite)  + ImageDeal[58].LeftBorder;
            //if(ImageDeal[Ysite].LeftBorder<3){ImageDeal[Ysite].LeftBorder = 3;}
            ImageDeal[Ysite].LeftBorder = ImageDeal[Ysite].RightBorder -  Half_Bend_Wide[Ysite]-11;
            if(ImageDeal[Ysite].LeftBorder<3){ImageDeal[Ysite].LeftBorder = 3;}
            ImageDeal[Ysite].Center = (ImageDeal[Ysite].RightBorder + ImageDeal[Ysite].LeftBorder) / 2;
        }
    }
        //准备进环  左线补线
    if (ImageFlag.image_element_rings_flag == 9)
    {
        for (int Ysite = 59; Ysite > ImageStatus.OFFLine; Ysite--)
        {
            ImageDeal[Ysite].Center = ImageDeal[Ysite].LeftBorder + Half_Road_Wide[Ysite];
        }
    }
}
void Element_Test(void) {
    if (  
        ImageStatus.Road_type != LeftCirque
        && ImageStatus.Road_type != RightCirque
//      && ImageStatus.Road_type != Barn_in  // 入库
//      && ImageStatus.Road_type != Ramp     // 坡道
        )
    {
//      ImageStatus.Road_type = 0;  // 重置道路类型
    }

    if (   ImageStatus.Road_type != Barn_in  // 圆环检测
        && ImageStatus.Road_type != RightCirque
        && ImageStatus.Road_type != LeftCirque
        )
    {
        Element_Judgment_Left_Rings();           // 左圆环检测
        Element_Judgment_Right_Rings();          // 右圆环检测
    }
}

void Element_Handle() {
    if (ImageFlag.image_element_rings == 1)
    
        Element_Handle_Left_Rings();          // 处理左圆环
    else if(ImageFlag.image_element_rings == 2)
        Element_Handle_Right_Rings();         // 处理右圆环
}
// 误差按权重重新整定
void GetDet() {
    float DetTemp = 0;
    int TowPoint = 26;
    float UnitAll = 0;

    // 根据道路类型确定前瞻
    if ((ImageStatus.Road_type == RightCirque || ImageStatus.Road_type == LeftCirque) &&
        ImageStatus.CirqueOff == 'F')
        TowPoint = 29;  // 圆环前瞻

    // 限制前瞻范围
    if (TowPoint < ImageStatus.OFFLine)
        TowPoint = ImageStatus.OFFLine + 1;
    if (TowPoint >= 49)
        TowPoint = 49;

    // 计算加权平均偏差
    if ((TowPoint - 5) >= ImageStatus.OFFLine) {
        // 前5行加权
        for (int Ysite = (TowPoint - 5); Ysite < TowPoint; Ysite++) {
            DetTemp = DetTemp + Weighting[TowPoint - Ysite - 1] * (ImageDeal[Ysite].Center);
            UnitAll = UnitAll + Weighting[TowPoint - Ysite - 1];
        }
        // 后5行加权
        for (Ysite = (TowPoint + 5); Ysite > TowPoint; Ysite--) {
            DetTemp += Weighting[-TowPoint + Ysite - 1] * (ImageDeal[Ysite].Center);
            UnitAll += Weighting[-TowPoint + Ysite - 1];
        }
        DetTemp = (ImageDeal[TowPoint].Center + DetTemp) / (UnitAll + 1);
    }
    else if (TowPoint > ImageStatus.OFFLine) {
        for (Ysite = ImageStatus.OFFLine; Ysite < TowPoint; Ysite++) {
            DetTemp += Weighting[TowPoint - Ysite - 1] * (ImageDeal[Ysite].Center);
            UnitAll += Weighting[TowPoint - Ysite - 1];
        }
        for (Ysite = (TowPoint + TowPoint - ImageStatus.OFFLine); Ysite > TowPoint; Ysite--) {
            DetTemp += Weighting[-TowPoint + Ysite - 1] * (ImageDeal[Ysite].Center);
            UnitAll += Weighting[-TowPoint + Ysite - 1];
        }
        DetTemp = (ImageDeal[Ysite].Center + DetTemp) / (UnitAll + 1);
    }
    else if (ImageStatus.OFFLine < 49) {
        for (Ysite = (ImageStatus.OFFLine + 3); Ysite > ImageStatus.OFFLine; Ysite--) {
            DetTemp += Weighting[-TowPoint + Ysite - 1] * (ImageDeal[Ysite].Center);
            UnitAll += Weighting[-TowPoint + Ysite - 1];
        }
        DetTemp = (ImageDeal[ImageStatus.OFFLine].Center + DetTemp) / (UnitAll + 1);
    }
    else {
        DetTemp = ImageStatus.Det_True;  // 如果出现OFFLine>50情况，保持上一次的偏差值
    }

    ImageStatus.Det_True = DetTemp;  // 此时的解算出来的平均图像偏差
    ImageStatus.TowPoint_True = TowPoint;  // 此时的前瞻
}
void PredictCenterLine() {
    // 使用前30行的点来计算变化率
    const int PREDICT_ROWS = 30;  // 用于预测的行数
    float sum_x = 0;
    float sum_y = 0;
    float sum_xy = 0;
    float sum_xx = 0;
    int valid_points = 0;

    // 计算有效的起始行
    int start_row = ImageStatus.OFFLine - PREDICT_ROWS;
    if (start_row < 0) start_row = 0;

    // 收集数据点用于线性回归
    for (int i = ImageStatus.OFFLine - 1; i >= start_row; i--) {
        if (CenterLine[i] > 0 && CenterLine[i] < LCDW) {
            sum_x += i;
            sum_y += CenterLine[i];
            sum_xy += i * CenterLine[i];
            sum_xx += i * i;
            valid_points++;
        }
    }

    // 如果有足够的有效点进行预测
    if (valid_points > 5) {
        // 计算线性回归系数
        float slope = (valid_points * sum_xy - sum_x * sum_y) / (valid_points * sum_xx - sum_x * sum_x);
        float intercept = (sum_y - slope * sum_x) / valid_points;

        // 使用线性回归结果进行预测
        for (int i = start_row - 1; i >= 0; i--) {
            float predicted = slope * i + intercept;
            // 限制预测值在图像范围内
            if (predicted < 0) predicted = 0;
            if (predicted >= LCDW) predicted = LCDW - 1;
            PredictedCenter[i] = round(predicted);
        }
    }
    
    // 在display_buffer上画出预测的中线
    for (int i = 0; i < LCDH; i++) {
        if (PredictedCenter[i] > 0 && PredictedCenter[i] < LCDW) {
            // 使用绿色(BGR: 0, 255, 0)标记预测中线
            display_buffer.at<cv::Vec3b>(i, PredictedCenter[i]) = cv::Vec3b(0, 255, 0);
            
            // 为了让线更粗一些，可以把周围的点也标记出来
            if (PredictedCenter[i] > 0) {
                display_buffer.at<cv::Vec3b>(i, PredictedCenter[i]-1) = cv::Vec3b(0, 255, 0);
            }
            if (PredictedCenter[i] < LCDW-1) {
                display_buffer.at<cv::Vec3b>(i, PredictedCenter[i]+1) = cv::Vec3b(0, 255, 0);
            }
        }
    }
}
// 修改drawBoundaryPoints函数，添加预测中线的绘制
void drawBoundaryPoints() {
    // 创建彩色显示缓冲区
    // 将二值化图像转换为彩色图像
    cv::cvtColor(compressed, display_buffer, cv::COLOR_GRAY2BGR);
    
    // 在彩色图像上画点
    for (int row = LCDH - 1; row >= 0; row--) {
        if (row < ImageStatus.OFFLine) {
            break;
        }
        
        if (ImageDeal[row].IsLeftFind == 'T') {
            int leftBoundary = ImageDeal[row].LeftBorder;
            cv::circle(display_buffer, cv::Point(leftBoundary, row), 1, cv::Scalar(255, 0, 0), -1); // 左边界为蓝色
        }
        if (ImageDeal[row].IsRightFind == 'T') {
            int rightBoundary = ImageDeal[row].RightBorder;
            cv::circle(display_buffer, cv::Point(rightBoundary, row), 1, cv::Scalar(0, 255, 0), -1); // 右边界为绿色
        }
        
        // 绘制实际中线
        cv::circle(display_buffer, cv::Point(ImageDeal[row].Center, row), 1, cv::Scalar(0, 0, 255), -1); // 中线为红色
    }
    
    // 绘制预测的中线
    if (EnablePrediction) {
        for (int row = ImageStatus.OFFLine - 1; row >= 0 && row >= ImageStatus.OFFLine - 10; row--) {
            cv::circle(display_buffer, cv::Point(PredictedCenter[row], row), 1, cv::Scalar(255, 255, 0), -1); // 预测中线为黄色
        }
    }
}

// 主图像处理函数
// 包含图像获取、预处理、边界提取、路径规划等完整处理流程
void ImageProcess() {
    // 初始化图像状态参数
    ImageStatus.OFFLine = 2;  // 设置截止行
    ImageStatus.WhiteLine = 0;  // 丢线计数器清零
    
    // 初始化图像边界数组
    for (int i = 0; i < 60; i++) {
        ImageDeal[i].IsRightFind = 'F';  // 右边界查找标志
        ImageDeal[i].IsLeftFind = 'F';   // 左边界查找标志
        ImageDeal[i].LeftBorder = 0;     // 左边界位置
        ImageDeal[i].RightBorder = 79;   // 右边界位置
        ImageDeal[i].Center = 40;        // 中线位置
        ImageDeal[i].Wide = 79;          // 赛道宽度
        ImageDeal[i].LeftBoundary = 0;   // 左边界初始位置
        ImageDeal[i].RightBoundary = 79; // 右边界初始位置
        ImageDeal[i].LeftBoundary_First = 0;    // 左边界首次搜索位置
        ImageDeal[i].RightBoundary_First = 79;  // 右边界首次搜索位置
    }
    
    // 图像处理性能计时开始
    auto capture_start = std::chrono::high_resolution_clock::now();
    
    // 获取压缩后的图像数据
    getCompressedImage();
    
    // 图像获取计时结束
    auto capture_end = std::chrono::high_resolution_clock::now();
    auto capture_duration = std::chrono::duration_cast<std::chrono::microseconds>(capture_end - capture_start);
    
    // 图像处理计时开始
    auto process_start = std::chrono::high_resolution_clock::now();
    
    // 执行图像处理流程
    DrawLinesFirst();      // 首次边界提取
    DrawLinesProcess();    // 边界处理
    Search_Border_OTSU(Pixle, LCDH, LCDW, LCDH - 2);// 迷宫法
    Element_Test();        // 元素检测
    DrawExtensionLine();   // 边界延伸
    Element_Handle();
    RouteFilter();         // 路径滤波
    GetDet();             // 获取偏差
    
    // 执行中线预测
    PredictCenterLine();   // 添加中线预测功能
    
    // 只在图传开启时绘制边界点
    if (config.image_trans) {
        drawBoundaryPoints();  // 绘制边界点
    }
    
    // 计算当前帧的舵机打角
    float current_deviation = ImageStatus.Det_True;
    current_angle = current_deviation;
    
    // 图像处理计时结束
    auto process_end = std::chrono::high_resolution_clock::now();
    auto process_duration = std::chrono::duration_cast<std::chrono::microseconds>(process_end - process_start);
    //负压启动
   // pwm_set_duty(ESC_PWM_1, 10);
   // pwm_set_duty(ESC_PWM_2, 10);
    
    // 输出处理时间统计信息
    std::cout << "图像获取时间: " << capture_duration.count() / 1000.0 << "ms, "
              << "图像处理时间: " << process_duration.count() / 1000.0 << "ms, "
              << "总时间: " << (capture_duration + process_duration).count() / 1000.0 << "ms" 
              <<"元素类型"<<ImageFlag.image_element_rings_flag<< std::endl;
}

// PID参数处理函数
// 从配置文件读取并更新PID控制器参数
void processUserInput(PIDController& pid) {
    std::ifstream file("../pid_params.txt");
    if (!file.is_open()) {
        std::cerr << "无法打开PID参数文件，使用默认值" << std::endl;
        return;
    }

    std::string line;
    while (std::getline(file, line)) {
        // 跳过空行和注释行
        if (line.empty() || line[0] == '/' || line[0] == '\n') {
            continue;
        }

        // 解析参数配置行
        size_t pos = line.find('=');
        if (pos != std::string::npos) {
            std::string key = line.substr(0, pos);
            std::string value = line.substr(pos + 1);
            
            // 提取参数值（去除注释部分）
            size_t comment_pos = value.find('/');
            if (comment_pos != std::string::npos) {
                value = value.substr(0, comment_pos);
            }

            // 清理参数值中的空白字符
            key.erase(0, key.find_first_not_of(" \t"));
            key.erase(key.find_last_not_of(" \t") + 1);
            value.erase(0, value.find_first_not_of(" \t"));
            value.erase(value.find_last_not_of(" \t") + 1);

            try {
                if (key == "Kp") config.Kp = std::stof(value);
                else if (key == "Ki") config.Ki = std::stof(value);
                else if (key == "Kd") config.Kd = std::stof(value);
                else if (key == "pwm") config.pwm = std::stoi(value);
                else if (key == "enable") config.enable = std::stoi(value);
                else if (key == "time_lock") config.time_lock = std::stof(value);
                else if (key == "dajiao") config.dajiao = std::stof(value);
                else if (key == "Kp_zhi") config.Kp_zhi = std::stof(value);
                else if (key == "Kd_zhi") config.Kd_zhi = std::stof(value);
                else if (key == "Kp_wan") config.Kp_wan = std::stof(value);
                else if (key == "Kd_wan") config.Kd_wan = std::stof(value);
                else if (key == "nts_flag") config.nts_flag = std::stof(value);
                else if (key == "show_flag") config.show_flag = std::stoi(value);
                else if (key == "image_trans") config.image_trans = std::stoi(value);
            } catch (const std::exception& e) {
                std::cerr << "解析参数 " << key << " 出错: " << e.what() << std::endl;
            }
        }
    }

    // 设置PID参数
    pid.Kp = config.Kp;
    pid.Ki = config.Ki;
    pid.Kd = config.Kd;
    
    // 更新全局变量
    time_lock = config.time_lock;
    dajiao = config.dajiao;
    nts_flag = config.nts_flag;
    show_flag = config.show_flag;

    // 输出读取的参数
    std::cout << "已加载PID参数：" << std::endl;
    std::cout << "Kp = " << pid.Kp << std::endl;
    std::cout << "Ki = " << pid.Ki << std::endl;
    std::cout << "Kd = " << pid.Kd << std::endl;
    std::cout << "PWM = " << config.pwm << std::endl;
    std::cout << "直弯分离参数：" << std::endl;
    std::cout << "Kp_zhi = " << config.Kp_zhi << std::endl;
    std::cout << "Kd_zhi = " << config.Kd_zhi << std::endl;
    std::cout << "Kp_wan = " << config.Kp_wan << std::endl;
    std::cout << "Kd_wan = " << config.Kd_wan << std::endl;
    std::cout << "图传功能：" << (config.image_trans ? "开启" : "关闭") << std::endl;

    file.close();
}

// 差速计算函数
void speed_dif_jun(void) {
    // 将舵机PWM值转换为角度
    // 舵机PWM值范围是0到SERVO_PWM_DUTY_MAX
    // 假设舵机的机械角度范围是±45度  1000 1170                   x                           35 = 200 / x
    float turn_angle = ((float)S3010_Duty - SERVO_MID_DUTY) / 5.571;  // 35度
    
    // 计算差速系数
    float v_dif = Speed_Diffen_xishu * tan(turn_angle * 3.1415926f / 180.0f) * 0.424f;

    if (v_dif < 0) {
        v_dif = -v_dif;
    }

    // 限制差速系数的范围
    if (v_dif > 0.8f) {
        v_dif = 0.8f;
    }

    if (turn_angle > 0) { // 右转
        Speed_Goal_l = Speed_Goal;
        Speed_Goal_r = (1 - v_dif) * Speed_Goal;
    }
    else if (turn_angle < 0) { //左转
        Speed_Goal_l = (1 - v_dif) * Speed_Goal;
        Speed_Goal_r = Speed_Goal;
    }
    else { // 直行
        Speed_Goal_l = Speed_Goal;
        Speed_Goal_r = Speed_Goal;
    }

    // 限制最小速度
    if (Speed_Goal_l < Speed_Goal * 0.2f) Speed_Goal_l = Speed_Goal * 0.2f;
    if (Speed_Goal_r < Speed_Goal * 0.2f) Speed_Goal_r = Speed_Goal * 0.2f;
}

// 清理函数
void cleanup() {
    // 先停止所有PWM输出
    pwm_set_duty(MOTOR1_PWM, 0);
    pwm_set_duty(MOTOR2_PWM, 0);
    pwm_set_duty(SERVO_PWM, 0);
    pwm_set_duty(ESC_PWM_1, 0);
    pwm_set_duty(ESC_PWM_2, 0);
    // 等待一小段时间确保PWM已经停止
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    
    // 关闭摄像头
    if (cap.isOpened()) {
        cap.release();
    }
    
    // 清除显示
    ips200_clear();
    
    std::cout << "清理完成，程序正常退出" << std::endl;
}

// 信号处理函数
void signal_handler(int signum) {
    std::cout << "\n收到信号 " << signum << "，开始清理..." << std::endl;
    should_exit = true;
    cleanup();
    exit(0);
}

// 全车
// 主函数
// 系统初始化
void init_system(void)
{
    // 注册信号处理函数
    signal(SIGINT, signal_handler);  // Ctrl+C
    signal(SIGTERM, signal_handler); // 终止信号
    signal(SIGSEGV, signal_handler); // 段错误
    
    // 注册程序退出时的清理函数
    atexit(cleanup);

    // 处理用户输入
    processUserInput(pid);  

    // 初始化IPS200显示屏
    ips200_init("/dev/fb0");  // 根据实际设备路径调整
    ips200_clear();  // 清屏
    
    // 初始化摄像头
    if (!initCamera()) {
        exit(-1);
    }
    
    // 获取PWM设备信息
    pwm_get_dev_info(MOTOR1_PWM, &motor_1_pwm_info);
    pwm_get_dev_info(MOTOR2_PWM, &motor_2_pwm_info);
    pwm_get_dev_info(SERVO_PWM, &servo_pwm_info);
   // pwm_get_dev_info((ESC_PWM_1,
    //pwm_get_dev_info
}

// 线程初始化
void init_threads(void)
{
    // 暂时只启动舵机控制定时器，用于调试
    std::cout << "正在启动舵机控制定时器..." << std::endl;
    pit_ms_init(10, servo_control_callback);
    std::cout << "舵机控制定时器已启动" << std::endl;
    // 电机控制定时器暂时注释掉
    pit_ms_init(5, motor_control_callback);
    //电调使用
    pit_ms_init(20,esc_control_callback);
}

// 网络初始化
void init_network(void)
{
    // 根据图传标志位决定是否初始化网络
    if (!config.image_trans) {
        std::cout << "图传功能已禁用" << std::endl;
        return;
    }
    
    sock = socket(AF_INET, SOCK_STREAM, 0);
    if (sock < 0) {
        std::cerr << "无法创建 socket" << std::endl;
        exit(-1);
    }

    struct sockaddr_in server_addr;
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(12345);
    server_addr.sin_addr.s_addr = inet_addr("192.168.5.229");

    if (connect(sock, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
        std::cerr << "连接失败" << std::endl;
        close(sock);
        exit(-1);
    }
    
    std::cout << "网络连接已建立，图传功能已启用" << std::endl;
}

// 主循环
void run_main_loop(void)
{
    while (!should_exit) {
        ImageProcess();
        
        // 只在图传开启时发送数据
        if (config.image_trans) {
            // 1. 发送帧同步标记
            const uint8_t frame_sync[2] = {0xAA, 0x55};
            if (send(sock, frame_sync, sizeof(frame_sync), 0) != sizeof(frame_sync)) {
                std::cerr << "发送帧同步标记失败" << std::endl;
                continue;
            }

            // 2. 发送图像数据 (80x60 BGR图像，包含边线)
            if (send(sock, display_buffer.data, LCDH * LCDW * 3, 0) != LCDH * LCDW * 3) {
                std::cerr << "发送图像数据失败" << std::endl;
                continue;
            }

            // 3. 发送编码器数据
            TransmitData data;
            data.deviation = ImageStatus.Det_True;
            data.encoder_left = encoder_left;
            data.encoder_right = encoder_right;
            
            if (send(sock, &data, sizeof(TransmitData), 0) != sizeof(TransmitData)) {
                std::cerr << "发送数据失败" << std::endl;
                continue;
            }
        }
        
        system_delay_ms(1);  // 增加延时到5ms，降低发送频率
    }
}